/* Includes ------------------------------------------------------------------*/
#include "CoppeliaSim.h"
#include "sys_log.h"
#include "datapool.h"
#include "gait.h"

#include <iostream>
/* Usr defines ---------------------------------------------------------------*/
using namespace std; 

_simObjectHandle_Type* Joint[4][3];
_simObjectHandle_Type* Body;
_simSignalHandle_Type* EulerAngle[3];

/* Founctions ----------------------------------------------------------------*/
uint32_t getSimTime();
/**
* @brief This is the main function for user.
*/
void Usr_Main()
{
    gait(state_gait_params[TROT], 0.0, 0.5, 0.5, 0.0);
    
}

/**
* @brief User can config simulation client in this function.
* @note  It will be called before entering the main loop.   
*/
void Usr_ConfigSimulation()
{
    Body = CoppeliaSim->Add_Object("Body", OTHER_OBJECT, { SIM_ORIENTATION | CLIENT_RO, SIM_VELOCITY | CLIENT_RO });
    
    Joint[LF][Abduction] = CoppeliaSim->Add_Object("LF_Abduction", JOINT, { SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW, SIM_FORCE | CLIENT_RO });
    Joint[LH][Abduction] = CoppeliaSim->Add_Object("LH_Abduction", JOINT, { SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW, SIM_FORCE | CLIENT_RO });
    Joint[RF][Abduction] = CoppeliaSim->Add_Object("RF_Abduction", JOINT, { SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW, SIM_FORCE | CLIENT_RO });
    Joint[RH][Abduction] = CoppeliaSim->Add_Object("RH_Abduction", JOINT, { SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW, SIM_FORCE | CLIENT_RO });

    Joint[LF][FrontPart] = CoppeliaSim->Add_Object("LF_F", JOINT, { SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW, SIM_FORCE | CLIENT_RO});
    Joint[LH][FrontPart] = CoppeliaSim->Add_Object("LH_F", JOINT, { SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW, SIM_FORCE | CLIENT_RO});
    Joint[RF][FrontPart] = CoppeliaSim->Add_Object("RF_F", JOINT, { SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW, SIM_FORCE | CLIENT_RO});
    Joint[RH][FrontPart] = CoppeliaSim->Add_Object("RH_F", JOINT, { SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW, SIM_FORCE | CLIENT_RO});
    
    Joint[LF][HindPart] = CoppeliaSim->Add_Object("LF_H", JOINT, {  SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW, SIM_FORCE | CLIENT_RO});
    Joint[LH][HindPart] = CoppeliaSim->Add_Object("LH_H", JOINT, { SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW, SIM_FORCE | CLIENT_RO});
    Joint[RF][HindPart] = CoppeliaSim->Add_Object("RF_H", JOINT, { SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW, SIM_FORCE | CLIENT_RO });
    Joint[RH][HindPart] = CoppeliaSim->Add_Object("RH_H", JOINT, { SIM_VELOCITY | CLIENT_RW, SIM_POSITION | CLIENT_RW, SIM_FORCE | CLIENT_RO });

    EulerAngle[yaw] = CoppeliaSim->Add_Object("car.YawAng", SIM_FLOAT_SIGNAL, { SIM_SIGNAL_OP | CLIENT_RO });
    EulerAngle[pitch] = CoppeliaSim->Add_Object("car.PitchAng", SIM_FLOAT_SIGNAL, { SIM_SIGNAL_OP | CLIENT_RO });
    EulerAngle[roll] = CoppeliaSim->Add_Object("car.RollAng", SIM_FLOAT_SIGNAL, { SIM_SIGNAL_OP | CLIENT_RO });
}

/**
* @brief These two function will be called for each loop.
*        User can set their message to send or read from sim enviroment.
*/
void Usr_SendToSimulation()
{
    Joint[LF][FrontPart]->obj_Target.angle_f =cmd_data[LF][FrontPart];
    Joint[LF][HindPart]->obj_Target.angle_f =  cmd_data[LF][HindPart];
    Joint[LF][Abduction]->obj_Target.angle_f = cmd_data[LF][Abduction];

    Joint[RF][FrontPart]->obj_Target.angle_f = cmd_data[RF][FrontPart];
    Joint[RF][HindPart]->obj_Target.angle_f =  cmd_data[RF][HindPart];
    Joint[RF][Abduction]->obj_Target.angle_f = cmd_data[RF][Abduction];

    Joint[LH][FrontPart]->obj_Target.angle_f = cmd_data[LH][FrontPart];
    Joint[LH][HindPart]->obj_Target.angle_f =  cmd_data[LH][HindPart];
    Joint[LH][Abduction]->obj_Target.angle_f = cmd_data[LH][Abduction];

    Joint[RH][FrontPart]->obj_Target.angle_f = cmd_data[RH][FrontPart];
    Joint[RH][HindPart]->obj_Target.angle_f =  cmd_data[RH][HindPart];
    Joint[RH][Abduction]->obj_Target.angle_f = cmd_data[RH][Abduction];
}

void Usr_ReadFromSimulation()
{
    fb_data[LF][Abduction]= Joint[LF][Abduction]->obj_Data.angle_f;
    fb_data[LF][FrontPart]= Joint[LF][FrontPart]->obj_Data.angle_f;
    fb_data[LF][HindPart]  = Joint[LF][HindPart]->obj_Data.angle_f;

    fb_data[RF][Abduction]= Joint[RF][Abduction]->obj_Data.angle_f;
    fb_data[RF][FrontPart]= Joint[RF][FrontPart]->obj_Data.angle_f;
    fb_data[RF][HindPart] = Joint[RF][HindPart]->obj_Data.angle_f;

    fb_data[RH][Abduction] = Joint[RH][Abduction]->obj_Data.angle_f;
    fb_data[RH][FrontPart]= Joint[RH][FrontPart]->obj_Data.angle_f;
    fb_data[RH][HindPart] = Joint[RH][HindPart]->obj_Data.angle_f;

    fb_data[LH][Abduction] = Joint[LH][Abduction]->obj_Data.angle_f;
    fb_data[LH][FrontPart] = Joint[LH][FrontPart]->obj_Data.angle_f;
    fb_data[LH][HindPart]  = Joint[LH][HindPart]->obj_Data.angle_f;

   fb_imu[roll]= EulerAngle[roll]->data;
    fb_imu[pitch] = EulerAngle[pitch]->data;
    fb_imu[yaw] = EulerAngle[yaw]->data;
}

/**
* @brief It's NOT recommended that user modefies this function.
*        Plz programm the functions with the prefix "Usr_". 
*/
int main(int argc, char *argv[])
{
    CoppeliaSim_Client *hClient = &CoppeliaSim_Client::getInstance();
    /*
        System Logger tool init.
    */
    std::cout << "[System Logger] Configuring... \n";
    SysLog->getMilliTick_regist(getSimTime);
    std::cout << "[System Logger] Logger is ready ! \n";

    /*
        Simulation connection init.
    */
    std::cout << "[CoppeliaSim Client] Connecting to server.. \n";
    while (!hClient->Start("127.0.0.1", 5000, 5, false)){};
    std::cout << "[CoppeliaSim Client] Successfully connected to server, configuring...\n";
    Usr_ConfigSimulation();
    std::cout << "[CoppeliaSim Client] Configure done, simulation is ready ! \n";

    while (1)
    {
        if (hClient->Is_Connected())
        {
            hClient->ComWithServer();
        }
        Usr_ReadFromSimulation();
    
        Usr_Main();
        
         Usr_SendToSimulation();
    }
}


uint32_t getSimTime()
{ 
    return 0;
}
/************************* END-OF-FILE SCUT-ROBOTLAB **************************/